#ifndef STATE_MACHINE_HPP
#define STATE_MACHINE_HPP

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <lifecycle_msgs/msg/transition.hpp>
#include <lifecycle_msgs/srv/change_state.hpp>
#include <lifecycle_msgs/srv/get_state.hpp>
#include <std_msgs/msg/bool.hpp>
#include <chrono>
#include <string>
#include <vector>
#include <map>
#include <memory>
#include <mutex>

namespace fangshe_sm {

    // 顶层状态机状态枚举
    enum class State {
        INIT,
        START_CENTER,
        WAIT_CENTER_DELAY,
        START_NAVIGATION,
        WAIT_NAV,
        START_MQTT,
        WAIT_MQTT_DELAY,
        START_ALIGNMENT,
        WAIT_ALIGN,
        START_GRIPPER,
        WAIT_GRIPPER,
        COMPLETED,
        ERROR // Optional, for handling failures
    };

    // 参数结构体
    struct MasterParams {
        std::string source_type = "neutron";
        std::string action_type = "grasp";
        double nav_timeout = 30.0;
        double align_timeout = 60.0;
        double gripper_timeout = 15.0;
        double delay_before_local_nav = 2.0;
        double delay_before_line_align = 2.0;

        std::string local_navigation_reached_topic = "/navigation/reached";
        std::string line_alignment_completed_topic = "/fine_alignment/completed";
        std::string gripper_completed_topic = "/gripper/completed";

        // 子节点参数映射
        std::map<std::string, std::map<std::string, std::string>> sub_node_params;
    };

    class StateMachine : public rclcpp::Node
    {
    public:
        explicit StateMachine(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
        ~StateMachine() = default;

    private:
        // 成员变量
        State current_state_ = State::INIT;
        MasterParams params_;

        // Lifecycle clients for managing sub-nodes
        // Use LifecycleNodeInterface which provides the client functionality
        std::map<std::string, rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::SharedPtr> lifecycle_clients_;

        // Parameter clients for setting sub-node parameters
        std::map<std::string, rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr> param_clients_;

        // Subscriptions for completion topics
        rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr nav_reached_sub_;
        rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr align_completed_sub_;
        rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr gripper_completed_sub_;

        // Timers
        rclcpp::TimerBase::SharedPtr delay_timer_;
        rclcpp::TimerBase::SharedPtr timeout_timer_;

        // Flags for completion topics
        bool nav_reached_ = false;
        bool align_completed_ = false;
        bool gripper_completed_ = false;

        // Mutex for shared data
        std::mutex state_mutex_;


        // --- Private Methods ---
        void declare_parameters();
        void load_parameters();
        void create_lifecycle_clients();
        void create_parameter_clients();
        void configure_and_activate_node(const std::string& node_name);
        void set_node_parameters(const std::string& node_name, const std::map<std::string, std::string>& params);

        // State Machine Logic
        void state_machine_logic();
        void handle_init_state();
        void handle_start_center_state();
        void handle_wait_center_delay_state();
        void handle_start_navigation_state();
        void handle_wait_nav_state();
        void handle_start_mqtt_state();
        void handle_wait_mqtt_delay_state();
        void handle_start_alignment_state();
        void handle_wait_align_state();
        void handle_start_gripper_state();
        void handle_wait_gripper_state();
        void handle_completed_state();

        // Topic Callbacks
        void nav_reached_callback(const std_msgs::msg::Bool::SharedPtr msg);
        void align_completed_callback(const std_msgs::msg::Bool::SharedPtr msg);
        void gripper_completed_callback(const std_msgs::msg::Bool::SharedPtr msg);

        // Timer Callbacks
        void delay_timer_callback();
        void timeout_timer_callback();

        // Helper to cancel timers
        void cancel_timers();
    };

} // namespace fangshe_sm

#endif // STATE_MACHINE_HPP